#include <WiFi.h>
#include <WiFiUdp.h>
#include "RoverC.h"

const char *ssid = "SXRobot20robot";
const char *password = "eLY5QJAX4gHqb5cp";

TFT_eSprite Disbuff = TFT_eSprite(&M5.Lcd);
WiFiServer server(80);

WiFiUDP Udp1;

void SetChargingCurrent( uint8_t CurrentLevel )
{
  Wire1.beginTransmission(0x34);
  Wire1.write(0x33);
  Wire1.write(0xC0 | ( CurrentLevel & 0x0f));
  Wire1.endTransmission();
}

void setup()
{
  M5.begin();
  M5.update();
  Wire.begin(0, 26, 10000);
  Serial.begin(115200);
  Serial.println();
  char x = 129;
  x = map(x, -127, -1, 255, 128);
  Serial.println(x);

  M5.Lcd.setRotation(1);
  M5.Lcd.setSwapBytes(false);
  //  Disbuff.createSprite(160, 80);
  //  Disbuff.setSwapBytes(true);
  //  Disbuff.fillRect(0, 0, 160, 20, Disbuff.color565(50, 50, 50));
  //  Disbuff.setTextSize(2);
  //  Disbuff.setTextColor(WHITE);
  //  Disbuff.setCursor(15, 35);
  //  Disbuff.pushSprite(0, 0);

  SetChargingCurrent(6);

  //Set device in STA mode to begin with
  WiFi.softAPConfig(IPAddress(192, 168, 4, 1),
                    IPAddress(192, 168, 4, 1),
                    IPAddress(255, 255, 255, 0));

  WiFi.softAP(ssid, password);
  IPAddress myIP = WiFi.softAPIP();
  M5.Lcd.print("AP IP address: ");
  M5.Lcd.println(myIP);
  server.begin();

  Udp1.begin(1003);
}

uint8_t I2CWrite1Byte( uint8_t Addr ,  uint8_t Data )
{
  Wire.beginTransmission(0x38);
  Wire.write(Addr);
  Wire.write(Data);
  return Wire.endTransmission();
}

uint8_t I2CWritebuff( uint8_t Addr,  uint8_t* Data, uint16_t Length )
{
  Wire.beginTransmission(0x38);
  Wire.write(Addr);
  for (int i = 0; i < Length; i++)
  {
    Wire.write(Data[i]);
  }
  return Wire.endTransmission();
}


int16_t speed_buff[4] = {0};
int8_t  speed_sendbuff[4] = {0};
uint32_t count = 0;
uint8_t IIC_ReState = I2C_ERROR_NO_BEGIN;

uint8_t Setspeed( int16_t Vtx, int16_t Vty, int16_t Wt)
{
  Wt = ( Wt > 100 )  ? 100 :  Wt;
  Wt = ( Wt < -100 ) ? -100 : Wt;

  Vtx = ( Vtx > 100 )  ? 100 :  Vtx;
  Vtx = ( Vtx < -100 ) ? -100 : Vtx;
  Vty = ( Vty > 100 )  ? 100 :  Vty;
  Vty = ( Vty < -100 ) ? -100 : Vty;

  Vtx = ( Wt != 0 ) ? Vtx *  (100 - abs(Wt)) / 100 : Vtx;
  Vty = ( Wt != 0 ) ? Vty *  (100 - abs(Wt)) / 100 : Vty;

  speed_buff[0] = Vty - Vtx - Wt ;
  speed_buff[1] = Vty + Vtx + Wt ;
  speed_buff[3] = Vty - Vtx + Wt ;
  speed_buff[2] = Vty + Vtx - Wt ;

  for (int i = 0; i < 4; i++)
  {
    speed_buff[i] = ( speed_buff[i] > 100 )  ? 100 :  speed_buff[i];
    speed_buff[i] = ( speed_buff[i] < -100 ) ? -100 : speed_buff[i];
    speed_sendbuff[i] = speed_buff[i];
  }
  return I2CWritebuff(0x00, (uint8_t*)speed_sendbuff, 4);
}

bool formatUdpData(char data[]) {
  if (!data[0] || sizeof(data) / sizeof(data[0] != 10) ) {
    return false;
  } else {
    return true;
  }

}

void loop()
{
  int udplength = Udp1.parsePacket();
  if ( udplength )
  {
    char udpData[udplength];
    Udp1.read( udpData, udplength);
    IPAddress udp_client = Udp1.remoteIP();
    Serial.println( formatUdpData(udpData) );
    /*
      接收到的数据形式如下：[8]
      {
        0xAB,
        舵机1（0x10）角度，舵机2（0x11）角度，
        左前轮（0x00）转速，右前轮（0x01）转速，左后轮（0x02）转速，右后轮（0x03）转速,
        0xAB
      }
      其中：
          舵机角度为0～180度
          电机速度为-127~127
          逆转速度原始数据为uint8_t，设定时需转化
    */
    if (( udpData[0] == 0xAB ) || udpData[7] == 0xAB)
    {
      for (int i = 0; i < 10; i++)
      {
        Serial.printf("%02x", udpData[i]);
        Serial.printf(" ");
      }
      Serial.println();
      Send_iic(0x00, udpData[3] );
      Send_iic(0x01, udpData[4] );
      Send_iic(0x02, udpData[5] );
      Send_iic(0x03, udpData[6] );
      uint8_t degree;
      degree = min(90, int(udpData[1]));
      degree = max(0, int(udpData[1]));
      Send_iic(0x10 , int(degree));
      degree = min(90, int(udpData[2]));
      degree = max(0, int(udpData[2]));
      Send_iic(0x11 , int(degree));
      delay(15);
    }
    else
    {
      IIC_ReState = Setspeed( 0  , 0 , 0 );
    }

  }
  count ++;
  if ( count > 100 )
  {
    count = 0;

    Disbuff.fillRect(0, 0, 160, 20, Disbuff.color565(50, 50, 50));
    Disbuff.setTextSize(1);
    Disbuff.setTextColor(WHITE);
    Disbuff.setCursor(5, 5);
    Disbuff.printf("%.2fV,%.2fmA,%d", M5.Axp.GetBatVoltage(), M5.Axp.GetBatCurrent(), IIC_ReState);
    Disbuff.pushSprite(0, 0);
  }
}
